import numpy as np

T_B_A = np.array([[0, -1, 0], [1, 0, 2], [0, 0, 1]])  # Transformation matrix from B to A

def p_BTop_A(T_B_A, B):
    Bb = np.array([B[0], B[1], 1])  # Convert B to homogeneous coordinates
    A = np.dot(T_B_A, Bb)
    return A[:2]  # Return only the x and y coordinates

def p_ATop_B(T_B_A, A):
    T_B_A_inv = np.linalg.inv(T_B_A)  # Inverse transformation matrix from A to B
    Aa = np.array([A[0], A[1], 1])  # Convert A to homogeneous coordinates

    B = np.dot(np.linalg.inv(T_B_A), Aa)
    return B[:2]  # Return only the x and y coordinates

if __name__ == "__main__":
    B = np.array([0, -1])  # Point in frame B
    A = p_BTop_A(T_B_A, B)  # Transform B to A
    print("Point in frame A:", A)
    print("Point in frame B:", p_ATop_B(T_B_A, A))  # Transform back to B